본 포스팅은 아래의 MathWorks 김종헌 프로님의 Path Planning 시리즈 중 아래의 비디오를 정리한 것입니다.

이 포스팅은 모바일 패스 플레이닝 시리즈의 세번째 파트의 네번째 시간으로 Navigation toolbox에서 제공하는 path planning algorithm 중 최적화 기반 경로 계획 방법에 대해 알아본다.

그림 1. Path Planning 알고리즘의 구분과 이번에 알아볼 Optimization-Based Path Planning 알고리즘

경로 계획 알고리즘의 분류에 대해 알고 싶은 사람은 시리즈 첫 번째 포스팅을 참고하시기 바란다. 그림 1 표의 가장 오른쪽의 설명과 같이 MATLAB에서 제공하는 최적화 기반 경로 계획 방법은 MPC, 즉 model predictive control을 이용하는 방법이다.

Model Predictive Control (MPC)

MPC solves a constrained optimization problem over a moving time horizon

그림 2. MPC의 작동 방법 개요

MPC는 제어 목표를 비용 함수로 인코딩하고 이를 최소화하는 제어 신호를 입력 및 출력 제약 조건에 따라 생성한다.

Model predictive control이라는 이름에서도 알수 있듯이 제어하려는 프로세스의 내부에 모델이 있고, 이 모델은 최적화 프로세스에서 미래에 일어날 가능성이 있는 일을 “예측”하는데 사용된다. MPC는 피드백 컨트롤러이기 때문에, 모델이 실제를 아주 정확하게 예측하지 않더라도 피드백을 통해 에러를 어느 정도 수정할 수 있으므로 예측 모델이 완벽할 필요는 없다.

How does MPC work?

그럼 실제로 MPC가 어떻게 동작하는지 살펴보자.

그림 3. 아주 단순화된 MPC의 구조와 제어 대상이 되는 플랜트(plant)

그림 3처럼 MPC 제어기를 보면 내부는 optimizer와 예측을 위한 plant 모델로 구성되어 있는 것을 알 수 있다. MPC 제어기 내부의 plant 모델은 계산 시간을 최소화하기 위해 실제 제어 대상 plant보다 좀 더 단순하게 표현하는 것이 일반적이다. 그리고, optimizer는 비용함수를 최소화하기 위한 최적화 알고리즘이라고 말할 수 있다.

그림 4. 플랜트가 자율주행차인 경우. 초록색으로 표시한 선이 차가 따라가야 할 reference 라인이고, MPC 제어는 reference와 차량중심까지의 거리를 입력으로 받아 조향각을 출력한다.

그림 4에서 처럼 이제 제어 대상을 모바일 로봇의 일종인 자율주행차라고 해보자. 이 차를 화면 좌상단의 도로에서 녹색으로 표시된 차선의 중앙을 따라가도록 하려고 한다. 이 차선 중앙이 바로 reference이다. 이제, 제어기의 출력은 차선 중심을 따라가게 하는 조향각 입력이다. 그리고 제어기로의 입력은 조향각 입력에 따라 차량이 움직여 바뀌고 있는 차선 중심에서부터 차량 중심까지의 거리인 횡방향 위치(lateral position)이다.

그림 5. 임의의 최초 제어 입력과 prediction horizon

그럼, 그림 5에서 처럼 최초 제어 입력을 만들어 보자. 이 때, 몇 번의 time step 동안에 대한 제어입력을 계획할지를 설정할 수 있는데 이 기간을 control horizon(보라색으로 표시)이라고 한다. 이렇게 지정한 임의의 초기 제어 입력과 차량의 거동예측 모델을 사용하면 일정 time step 이후에 내 로봇이 어디에 존재하는지 예측할 수 있다. 이로써 예측되게 되는 plant의 시간 범위를 prediction horizon(주황색으로 표시)이라고 한다.

그러면, 그림 5에서와 같이 예측된 거동에 대해 optimizer는 비용을 계산할 수 있게 된다. 이 예제에서 비용은 크게 두가지로 구성한다. 첫 번째 비용 요소는 각 시간 단계에서 reference와의 횡방향 오차이다. 이것은 그림 6에서 보는 것과 같이 설명할 수 있다.

그림 6. reference와의 횡방향 오차

그리고 두 번째 비용 요소는 control horizon의 조향 입력의 변화량인 $\Delta U$이다. 다시 말하면, 제어를 위해 노력한 양을 두 번째 비용으로 삼는것이다.

그림 7. 조향 입력의 변화

이 둘을 이용하면 최종적으로 아래 그림 8과 같이 비용함수를 구성할 수 있게 된다. 이렇게 되면 각 단계에서 최소한의 노력으로 최대한 빨리 그리고 안정적으로 횡방향 오차를 줄일 수 있게 된다.

그림 8. MPC 제어기의 목적 함수 선정 예시

그래서, 초기 입력에 대한 비용값을 계산한 값이 50이라고 하면 Optimizer에서는 이 비용함수를 최소화할 수 있도록 최적화 솔버를 이용하여 더 나은 값을 찾기 위해 계속 계산을 진행한다. 두번째는 비용값이 45로 줄었으며, 한번 더 계산을 진행하였더니 비용값이 20이 될 수 있을 것이다.

그림 9. MPC 제어기의 목적함수 최적화를 통한 경로 수정 예시

계산을 이렇게 계속하다보면, 몇가지 기준에 따르는 솔버 종료조건에 따라 더 이상 비용을 줄이기 어려운 최적 제어 값이 얻어지고 제어기는 이 제어 입력을 최종적으로 출력하게 된다.

Why use MPC?

그렇다면 어떤 이유로 MPC를 사용하는 것일까?

Preview Capability

첫번째로, 이름 그대로 prediction이 가능하다. 미래의 거동을 고려한 제어 입력으로 장기적으로 시스템을 안정되게 제어할 수 있다.

Control of MIMO Systems & Coupled Dynamics

두 번째로, MIMO 시스템을 제어할 수 있다. 모바일 로봇이 만약 매우 빠른 속도로 이동한다면, 조향입력이 커질 때는 속도를 낮춰야만 안정되게 곡선 경로를 추종할 수 있기 때문에 종방향과 횡방향 제어를 동시에 고려해야 한다. 다시 말해, 빠른 속도로 이동하는 로봇 중 하나인 자율주행 등에 적합한 제어 방식이다.

Multiple control objectives

셋째로, 다수의 제어 목표를 고려할 수 있다. 정해진 경로를 최대한 벗어나지 않으면서도, 주행 안정성을 보장하고, 외부 물체와의 충돌도 고려하는 등 비용함수의 어떻게 조합하느냐에 따라 여러 제어 목표를 한번에 고려하는 제어입력을 생성할 수 있다.

Dynamically feasible planning

네 번째로, 내부에 로봇의 거동 예측 모델을 사용함으로써, 동역학적으로 추종가능한 계획을 수립할 수 있다.

Constraints of inputs, outputs, states

마지막으로는, 입력, 출력, 혹은 시스템의 예측 상태에 대해 제약조건 준수하도록 할 수 있다는 장점도 있다.

Two approachese for the path tracking

그럼 왜 MPC는 모델 예측 제어라는 이름을 가진 제어기 인데 path planner로 사용된다고 얘기하는 것일까?

One-Level

만약, MPC 제어기에 로봇의 위치와 목표 위치를 제공해서 목표위치에 도달할 때까지의 waypoint들을 출력하도록 하거나 시간에 따른 속도/조향각 등의 제어명령을 출력하게 MPC 제어기를 구성하면 global path planner로서 mpc를 사용하는 것이 된다.

그림 10. MPC를 이용해 Global path planner로 사용하는 경우

이 때, 주변 장애물 정보를 제공하고 이 장애물과의 거리가 음수가 되지 않게 제약조건을 구성하면 장애물과의 충돌을 회피하는 경로를 생성할 수 있다.

하지만, 이 방법은 최적화 해법이 초기치에 민감하다는 점을 고려하면 문제가 복잡해지는 경우, 해가 수렴하지 않고 적절한 경로를 얻어내지 못할 수도 있다.

Two-Level

첫 번째 포스팅에서 언급했던 것처럼 Path planning을 하는 시점에서 모든 정보를 알수도 없고, 복잡한 실제 환경에서의 path planning을 수행하기 위해 일반적으로 global path planning과 local path planning의 조합인 계층구조적인 path planner를 사용하게 된다.

그림 11. Global path planner를 사용하여 생성한 경로를 MPC가 추종하도록 2단계로 설계하는 경우

만약 그림 11과 같이 탐색 기반 혹은 샘플링 기반의 global path planner를 사용하여 생성한 경로를 추종하도록 MPC를 설계한다면 이전 단계에서 생성된 경로가 MPC 제어기의 초기치 역할을 하여 계산 속도도 빠르고 안정적으로 수렴하는데 도움이 된다. 이를 통해 초기 경로계획 시점에서 고려하지 못한 주변 물체와의 충돌도 피하면서 부드러우면서도 안정적으로 로봇을 제어할 수 있는 최적 경로를 얻어낼 수 있다.

Ex) Truck-trailer Reverse Parking: NLMPC Formulation

예를 들어 아래 그림 12에서와 같이 trailer가 달린 트럭을 후진주차 하는 경로를 생성하는 문제에 대해, MPC가 global path planner 자체로 사용되는 경우와 global path planner는 탐색 기반 혹은 샘플링 기반으로 만들고 이를 MPC가 추종하는 경우를 각각 알아보도록 하자.

그림 12. trailer가 달린 트럭의 kinematics
출처: MathWorks

우선, Truck-trailer 모델이 필요하다. 일반적인 모바일 로봇보다는 훨씬 복잡한 kinematics를 가지고 있는 것을 알수 있다. 두 바디 사이의 관계를 수식화하고 이를 state space 형태로 모델링하면 그림 12의 오른쪽과 같다.

그림 13. MPC 제어기를 이용한 제어 구조

MPC 제어기의 입출력 및 전반적인 제어 구조는 그림 13과 같다. 예측 모델에서 현재 상태는 차량의 위치와 각 바디의 방위각을 입력받아 계산하고, 최종적으로 제어기가 출력하는 명령은 속도와 조향각으로 설정한다. 아래 식 (1)의 one level 모델의 비용함수를 보면, 현재의 pose와 목표 주차 상태의 pose 사이의 차이가 최소화되도록 구성한 것을 알 수 있다.

\[w_1|s(d)-s_{target}|^2\]

이 때, 제약조건으로 주변 물체와 일정 수준 이상의 안전거리를 유지해야하며, 속도는 안전을 위해 후진 vmin, 전진 vmax 범위로 유지하고, 가속도는 제어의 안정성을 위해 특정 가속도 범위 이내로 유지하도록 정의하였다.

\[\]

Technical Resources

정리하면 OOO 에 대해 알아보았다. Path Planning 분야 외의 로봇 개발을 위한 다른 내용이 궁금하다면 아래의 매스웍스 코리아에서 제공하는 모바일 로봇틱스 그리고 자율주행 웹 포털을 통해 정보를 얻어갈 수 있으니 참고하기 바란다.

MATLAB Mobile Robotics Web Portal

그림 29. MATLAB을 이용한 육상 이동 로봇 개발 Web Portal (링크)

MATLAB ADAS Web Portal

그림 30. MATLAB을 이용한 자율주행/ADAS 개발 Web Portal (링크)

MATLAB Onramp Series

매트랩 기초 사용법을 학습하고 싶은 경우 MathWorks 홈페이지 내의 Onramp 라는 무료 트레이닝 코스를 활용할 수 있다. Onramp는 웹상으로 진행하는 온라인 무료 교육으로, 컴퓨터에 매트랩을 설치 할 필요 없이 온라인으로 매트랩 관련된 여러 기초 내용을 학습할 수 있다.

그림 31. MATLAB을 무료로 배울 수 있는 Onramp 시리즈 (링크)